#include <pluginlib/class_list_macros.h>
#include <bipedRobin_local_planner/bipedRobin_planner_ros.h>
#include <base_local_planner/goal_functions.h>
#include <boost/thread.hpp>

#include <footstep_planner/helper.h>
#include <footstep_planner/FootstepPlanner.h>
#include <footstep_planner/State.h>

//register this planner as a BaseLocalPlanner plugin
PLUGINLIB_DECLARE_CLASS(bipedRobin_local_planner, BipedRobinPlannerROS, bipedRobin_local_planner::BipedRobinPlannerROS, nav_core::BaseLocalPlanner)

namespace bipedRobin_local_planner {
	void BipedRobinPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) {
		if(!initialized_){
			tf_ = tf;
			costmap_ros_ = costmap_ros;

			ros::NodeHandle gn;
			ros::NodeHandle pn("~/" + name);

			g_plan_pub_ = pn.advertise<nav_msgs::Path>("global_plan", 1);
			l_plan_pub_ = pn.advertise<nav_msgs::Path>("local_plan", 1);
			ivMapPub = pn.advertise<nav_msgs::OccupancyGrid>("local_map", 10);
			stepsLeftInBufferSub = gn.subscribe<std_msgs::UInt8>("stepsLeftInBuffer", 10, &BipedRobinPlannerROS::stepsLeftCallback, this);
			ivFootstepIncSrv = gn.serviceClient<bipedRobin_msgs::StepTarget3DService>("footstep3DInc_srv");

			pn.param("prune_plan", prune_plan_, true);

			ivIdFootRight = "/r_sole";
			ivIdFootLeft = "/l_sole";
			ivIdMapFrame = "/odom";
			pn.param("rfoot_frame_id", ivIdFootRight, ivIdFootRight);
			pn.param("lfoot_frame_id", ivIdFootLeft, ivIdFootLeft);
			pn.param("map_frame_id", ivIdMapFrame, ivIdMapFrame);
			pn.param("planning_distance", ivPlanningDistance, 1.0);
			
			if (!initializeNavigator()) {
				ROS_ERROR("Error during initialization of local footstep planner");
				return;
			}
				
			initialized_ = true;
		} else{
			ROS_WARN("This planner has already been initialized, doing nothing.");
		}
	}

	bool BipedRobinPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){
		if(!initialized_){
		  ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
		  return false;
		}

		std::vector<geometry_msgs::PoseStamped> local_plan;
		tf::Stamped<tf::Pose> global_pose;
		if(!costmap_ros_->getRobotPose(global_pose)) return false;
		
		std::vector<geometry_msgs::PoseStamped> transformed_plan;
		//get the global plan in our frame
		if(!base_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan)) {
		  ROS_WARN("Could not transform the global plan to the frame of the controller");
		  return false;
		}

		//now we'll prune the plan based on the position of the robot
		if(prune_plan_) {
			base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
		}
			
		//we also want to clear the robot footprint from the costmap we're using
		costmap_ros_->clearRobotFootprint();

		//if the global plan passed in is empty... we won't do anything
		if(transformed_plan.empty()) {
			return false;
		}

		/*//Print PLAN
		for (unsigned i=0; i<transformed_plan.size(); i++) {
			tf::Stamped<tf::Pose> point;
			tf::poseStampedMsgToTF(transformed_plan.at(i), point);
			ROS_INFO("%.2f ", tf::getYaw(point.getRotation()));
		}*/

		{
			boost::mutex::scoped_lock lock(localPlan_mutex_);
		
			//ROS_INFO("globale pose %f %f", global_pose.getOrigin().x(), global_pose.getOrigin().y());
			
			// get the goal position
			tf::Stamped<tf::Pose> point;
			double distance;
			unsigned int i = transformed_plan.size();
			do  {
				i--;
				tf::poseStampedMsgToTF(transformed_plan.at(i), point);
				tf::Transform delta = global_pose.inverse() * point;
				distance = sqrt(pow(delta.getOrigin().x(), 2.0) + pow(delta.getOrigin().y(), 2.0));
			} while (distance > ivPlanningDistance && i >= 0);
			
			tf::Stamped<tf::Pose> goal_point;
			tf::poseStampedMsgToTF(transformed_plan.at(i), goal_point);
			tf::Stamped<tf::Pose> temp_point;
			tf::poseStampedMsgToTF(transformed_plan.at(i-1), temp_point);
			
			ivLocalGoalX = goal_point.getOrigin().getX();
			ivLocalGoalY = goal_point.getOrigin().getY();
			ivLocalGoalTheta = atan2(ivLocalGoalY-temp_point.getOrigin().getY(), ivLocalGoalX-temp_point.getOrigin().getX()) ; //+ 1.57
			
			
			ivPlanSteps = true;

			// check if there are steps in the local plan and if the robot is ready to get a new step
			
			//if (ivStepsToStart.size()) ROS_INFO("Listengroese rauslesen %d", ivStepsToStart.size());
			if (ivStepsToStart.size() > 0 && ivSendStep>0) { 
				bipedRobin_msgs::StepTarget3DService stepSrv;
				bipedRobin_msgs::StepTarget3D footstep;
				
				footstep_planner::State supportStepPosition;
				const footstep_planner::State nextStepPosition = ivStepsToStart.front();
				
				ivStepsToStart.pop_front();
				
				if (nextStepPosition.getLeg() == footstep_planner::LEFT) {
					supportStepPosition = ivRightLegCurrent;
					ivLeftLegCurrent = nextStepPosition;
				} else {
					supportStepPosition = ivLeftLegCurrent;
					ivRightLegCurrent = nextStepPosition;
				}
					
					
				getFootstepFromState(supportStepPosition, nextStepPosition, &footstep);
				
				
				stepSrv.request.step = footstep;
				
				ivFootstepIncSrv.call(stepSrv);
				
				
				ROS_INFO("current step to go: %f %f %f %d %d", footstep.pose.position.x, footstep.pose.position.y, tf::getYaw(footstep.pose.orientation), footstep.leg, ivStepsToStart.size());
				
				ivSendStep--;
			}
		}
			

		/*if (ivFootstepPlanningMutex.try_lock()) {
			updateMapToPlanner();
			ivPlanner.setStart(ivLeftLegStart, ivRightLegStart);
			ivPlanner.setGoal(goal_x, goal_y, goal_th);		
			ivFootstepPlanningThread = new boost::thread(boost::bind(&BipedRobinPlannerROS::planningThread,this));
		} else {
			ROS_INFO("Planner is still looking for steps to the local goal");
		}*/

		//cmd_vel.linear.x = 0.0;
		//cmd_vel.linear.y = 0.0;
		//cmd_vel.angular.z = 0.0;
		//ROS_INFO("A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);

		//publish information to the visualizer
		base_local_planner::publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
		base_local_planner::publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
		return true;
	}

	bool BipedRobinPlannerROS::isGoalReached(){
		if(!initialized_){
			ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
			return false;
		}

		{
			boost::mutex::scoped_lock lock(localPlan_mutex_);
			//return (ivStepsToStart.size() == 1);
		}
		//TODO
		//return base_local_planner::goalPositionReached(global_pose, goal_x, goal_y, xy_goal_tolerance_) && base_local_planner::goalOrientationReached(global_pose, goal_th, yaw_goal_tolerance_);
		return false;
	}
	
	void BipedRobinPlannerROS::stepsLeftCallback(const std_msgs::UInt8 stepsLeftInBuffer) {
		if(stepsLeftInBuffer.data == 0){ //if the step buffer or the robot is empty
			//ros::Duration(0.8).sleep(); //wait a little bit shorter than one step time
			ROS_INFO (" Step needed");
			ivSendStep++;
		}
	}
	
	void  BipedRobinPlannerROS::getFootstepFromState(const footstep_planner::State& from,  const footstep_planner::State& to, bipedRobin_msgs::StepTarget3D* footstep)  {
		tf::Pose fromPose (tf::createQuaternionFromYaw(from.getTheta()), tf::Point(from.getX(), from.getY(), 0.0));
		tf::Pose toPose (tf::createQuaternionFromYaw(to.getTheta()), tf::Point(to.getX(), to.getY(), 0.0));
		
		tf::Transform step = fromPose.inverse() * toPose;  // get footstep to reach 'to' from 'from'

		//ROS_INFO("from step : %f %f %f %d", from.getX(), from.getY(), from.getTheta(), from.getLeg());
		//ROS_INFO("to step : %f %f %f %d", to.getX(), to.getY(), to.getTheta(), to.getLeg());
		
		// set the footstep
		footstep->pose.position.x = step.getOrigin().x();
		footstep->pose.position.y = step.getOrigin().y();
		footstep->pose.position.z = step.getOrigin().z();
		footstep->pose.orientation.x = step.getRotation().x();
		footstep->pose.orientation.y = step.getRotation().y();
		footstep->pose.orientation.z = step.getRotation().z();
		footstep->pose.orientation.w = step.getRotation().w();
		if (to.getLeg() == footstep_planner::LEFT) {
			footstep->leg = humanoid_nav_msgs::StepTarget::left;
		} else {
			footstep->leg = humanoid_nav_msgs::StepTarget::right;
		}
		
		//ROS_INFO("delta step: %f %f %d", footstep->pose.position.x, footstep->pose.position.y, footstep->leg);
	}
			
	bool BipedRobinPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan){
		if(!initialized_){
			ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
			return false;
		}

		//reset the global plan
		global_plan_.clear();
		global_plan_ = orig_global_plan;
		
		//TODO stop planning and reset local plan as well
		
		return true;
	}

	void BipedRobinPlannerROS::updateMapToPlanner() {
		nav_msgs::OccupancyGrid::Ptr occGrid(new nav_msgs::OccupancyGrid());

		costmap_2d::Costmap2D costmap;
		costmap_ros_->getCostmapCopy(costmap);

		const unsigned char* charMap = costmap.getCharMap();
		for(unsigned int i = 0; i < costmap_ros_->getSizeInCellsX()*costmap_ros_->getSizeInCellsY(); i++) {
			if(charMap[i] == costmap_2d::LETHAL_OBSTACLE)
				occGrid->data.push_back(100);
			else if(charMap[i] == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
				occGrid->data.push_back(50);
			else if(charMap[i] == costmap_2d::NO_INFORMATION)
				occGrid->data.push_back(-1);
			else 
				occGrid->data.push_back(0); 
		}

		occGrid->header.frame_id = costmap_ros_->getGlobalFrameID();
		occGrid->info.resolution=costmap_ros_->getResolution();
		occGrid->info.width=costmap_ros_->getSizeInCellsX();
		occGrid->info.height=costmap_ros_->getSizeInCellsY();
		occGrid->info.origin.position.x=costmap.getOriginX();
		occGrid->info.origin.position.y=costmap.getOriginY();

		ivMapPub.publish(occGrid); //just for visualization

		gridmap_2d::GridMap2DPtr map(new gridmap_2d::GridMap2D(occGrid));
		ivIdMapFrame = map->getFrameID();
		ivPlanner.updateMap(map);
	}

	bool BipedRobinPlannerROS::initializeNavigator() {
		try {
			tf::StampedTransform transOdomLeft;
			tf::StampedTransform transOdomRight;
			
			const ros::Time now = ros::Time::now();
			
			tf_->waitForTransform(ivIdMapFrame, ivIdFootLeft, now, ros::Duration(1.0));
			tf_->lookupTransform(ivIdMapFrame, ivIdFootLeft, now, transOdomLeft);
			ivLeftLegStart.setX(transOdomLeft.getOrigin().x());
			ivLeftLegStart.setY(transOdomLeft.getOrigin().y());
			ivLeftLegStart.setTheta(tf::getYaw(transOdomLeft.getRotation()));
			ivLeftLegStart.setLeg(footstep_planner::LEFT);
			ivLeftLegCurrent = ivLeftLegStart;
			ROS_INFO("Initialized left feet at %.3f %.3f %.3f", transOdomLeft.getOrigin().x(), transOdomLeft.getOrigin().y(), tf::getYaw(transOdomLeft.getRotation()));

			tf_->waitForTransform(ivIdMapFrame, ivIdFootRight, now, ros::Duration(1.0));
			tf_->lookupTransform(ivIdMapFrame, ivIdFootRight, now, transOdomRight);
			ivRightLegStart.setX(transOdomRight.getOrigin().x());
			ivRightLegStart.setY(transOdomRight.getOrigin().y());
			ivRightLegStart.setTheta(tf::getYaw(transOdomRight.getRotation()));
			ivRightLegStart.setLeg(footstep_planner::RIGHT);
			ivRightLegCurrent = ivRightLegStart;
			ROS_INFO("Initialized right feet at %.3f %.3f %.3f", transOdomRight.getOrigin().x(), transOdomRight.getOrigin().y(), tf::getYaw(transOdomRight.getRotation()));
			
			ivPlanSteps = false;
			ivFootstepPlanningThread = new boost::thread(boost::bind(&BipedRobinPlannerROS::planningThread,this));
			
			ivSendStep = true;

		} catch (tf::TransformException ex) {
			ROS_ERROR("%s", ex.what());
			return false;
		}
		return true;
	}
	
	void BipedRobinPlannerROS::planningThread(void) {
		
		ROS_INFO ("Footstep Planning Task has started");
		
		do {
			if (ivPlanSteps) {
				

				
			        {
					boost::mutex::scoped_lock lock(planner_mutex_);
					
					// update planner infos
					updateMapToPlanner();
					ivPlanner.setStart(ivLeftLegStart, ivRightLegStart);
					ivPlanner.setGoal(ivLocalGoalX, ivLocalGoalY, ivLocalGoalTheta);						
				}	

				// replan path
				ivPlanner.replan();
	
				// set interruption point
				try {
					boost::this_thread::interruption_point();
				} catch (const boost::thread_interrupted&){
					ROS_INFO("Planning Task was interrupted. Planner is starting again.");
					continue; //just start a replan
				}
				
				//check if new plan is realy a new plan
				footstep_planner::state_iter_t to_planned = ivPlanner.getPathBegin();
				footstep_planner::state_iter_t to_planned2 = ivPlanner.getPathBegin();
				if (to_planned == ivPlanner.getPathEnd()) {
					ROS_ERROR("No local plan found or steps are planned till goal");
					continue;  //just start a replan
				}
				
				{
					boost::mutex::scoped_lock lock(localPlan_mutex_);
					ROS_INFO(" 1234 current support leg: %f %f %f %d", to_planned.base()->getX(), to_planned.base()->getY(), to_planned.base()->getTheta(), to_planned.base()->getLeg());
						
					
					//while(to_planned2!= ivPlanner.getPathEnd()) {
					//	ROS_INFO("                               local plan: %f %f %f %d", to_planned2.base()->getX(), to_planned2.base()->getY(), to_planned2.base()->getTheta(), to_planned2.base()->getLeg());
					//	to_planned2++;
					//}
					
					//add steps to the local plan
					while (ivStepsToStart.size() < 3 && to_planned!= ivPlanner.getPathEnd()) {
						to_planned++;
						ivStepsToStart.push_back(*to_planned.base());
						
						ROS_INFO(" 1234 step putted in buffer: %f %f %f %d", to_planned.base()->getX(), to_planned.base()->getY(), to_planned.base()->getTheta(), to_planned.base()->getLeg());
						
						if (to_planned->getLeg() == 0) ivRightLegStart = *to_planned.base();
						else ivLeftLegStart = *to_planned.base();			
					}
					//ROS_INFO("Listengroese reinschreiben %d", ivStepsToStart.size());
				}
				
			}
			ros::Duration(0.1).sleep();
		} while(true) ;
		
		ROS_INFO ("Footstep Planning Task has terminated");
		ivFootstepPlanningMutex.unlock();
	}	
};
